/**
 * @ Author: luoqi
 * @ Create Time: 2024-03-14 10:53
 * @ Modified by: luoqi
 * @ Modified time: 2025-05-18 23:30
 * @ Description:
 */
#include "qlink.h"

static const uint8_t QLINK_PACK_HEAD[2] = { 0x4C, 0x51 };

static const uint16_t crc16tab[256] = {
    0x0000,0x1021,0x2042,0x3063,0x4084,0x50a5,0x60c6,0x70e7,
    0x8108,0x9129,0xa14a,0xb16b,0xc18c,0xd1ad,0xe1ce,0xf1ef,
    0x1231,0x0210,0x3273,0x2252,0x52b5,0x4294,0x72f7,0x62d6,
    0x9339,0x8318,0xb37b,0xa35a,0xd3bd,0xc39c,0xf3ff,0xe3de,
    0x2462,0x3443,0x0420,0x1401,0x64e6,0x74c7,0x44a4,0x5485,
    0xa56a,0xb54b,0x8528,0x9509,0xe5ee,0xf5cf,0xc5ac,0xd58d,
    0x3653,0x2672,0x1611,0x0630,0x76d7,0x66f6,0x5695,0x46b4,
    0xb75b,0xa77a,0x9719,0x8738,0xf7df,0xe7fe,0xd79d,0xc7bc,
    0x48c4,0x58e5,0x6886,0x78a7,0x0840,0x1861,0x2802,0x3823,
    0xc9cc,0xd9ed,0xe98e,0xf9af,0x8948,0x9969,0xa90a,0xb92b,
    0x5af5,0x4ad4,0x7ab7,0x6a96,0x1a71,0x0a50,0x3a33,0x2a12,
    0xdbfd,0xcbdc,0xfbbf,0xeb9e,0x9b79,0x8b58,0xbb3b,0xab1a,
    0x6ca6,0x7c87,0x4ce4,0x5cc5,0x2c22,0x3c03,0x0c60,0x1c41,
    0xedae,0xfd8f,0xcdec,0xddcd,0xad2a,0xbd0b,0x8d68,0x9d49,
    0x7e97,0x6eb6,0x5ed5,0x4ef4,0x3e13,0x2e32,0x1e51,0x0e70,
    0xff9f,0xefbe,0xdfdd,0xcffc,0xbf1b,0xaf3a,0x9f59,0x8f78,
    0x9188,0x81a9,0xb1ca,0xa1eb,0xd10c,0xc12d,0xf14e,0xe16f,
    0x1080,0x00a1,0x30c2,0x20e3,0x5004,0x4025,0x7046,0x6067,
    0x83b9,0x9398,0xa3fb,0xb3da,0xc33d,0xd31c,0xe37f,0xf35e,
    0x02b1,0x1290,0x22f3,0x32d2,0x4235,0x5214,0x6277,0x7256,
    0xb5ea,0xa5cb,0x95a8,0x8589,0xf56e,0xe54f,0xd52c,0xc50d,
    0x34e2,0x24c3,0x14a0,0x0481,0x7466,0x6447,0x5424,0x4405,
    0xa7db,0xb7fa,0x8799,0x97b8,0xe75f,0xf77e,0xc71d,0xd73c,
    0x26d3,0x36f2,0x0691,0x16b0,0x6657,0x7676,0x4615,0x5634,
    0xd94c,0xc96d,0xf90e,0xe92f,0x99c8,0x89e9,0xb98a,0xa9ab,
    0x5844,0x4865,0x7806,0x6827,0x18c0,0x08e1,0x3882,0x28a3,
    0xcb7d,0xdb5c,0xeb3f,0xfb1e,0x8bf9,0x9bd8,0xabbb,0xbb9a,
    0x4a75,0x5a54,0x6a37,0x7a16,0x0af1,0x1ad0,0x2ab3,0x3a92,
    0xfd2e,0xed0f,0xdd6c,0xcd4d,0xbdaa,0xad8b,0x9de8,0x8dc9,
    0x7c26,0x6c07,0x5c64,0x4c45,0x3ca2,0x2c83,0x1ce0,0x0cc1,
    0xef1f,0xff3e,0xcf5d,0xdf7c,0xaf9b,0xbfba,0x8fd9,0x9ff8,
    0x6e17,0x7e36,0x4e55,0x5e74,0x2e93,0x3eb2,0x0ed1,0x1ef0
};

static uint16_t _crc16(const uint8_t *buf, int len) {
    if(!buf) {
        return 0;
    }
    int counter;
    uint16_t crc = 0;
    for(counter = 0; counter < len; counter++) {
        crc = (crc << 8) ^ crc16tab[((crc >> 8) ^ *buf++) & 0x00FF];
    }
    return crc;
}

static uint16_t _id_calc(const char *name)
{
    if(!name) {
        return 0;
    }
    uint16_t hash = 5381;
    int c;
    while((c = *name++)) {
        hash = ((hash << 5) + hash) + c; /* hash * 33 + c */
    }
    return hash;
}

static inline void _list_insert(QLinkList *list, QLinkList *node)
{
    if(!list || !node) {
        return;
    }

    list->next->prev = node;
    node->next = list->next;

    list->next = node;
    node->prev = list;
}

static inline void _list_remove(QLinkList *node)
{
    if(!node || !node->next || !node->prev) {
        return;
    }

    if((node->next == node) && (node->prev == node)) {
        return;
    }

    node->next->prev = node->prev;
    node->prev->next = node->next;

    node->next = QNULL;
    node->prev = QNULL;
}

static inline void _list_init(QLinkList *list)
{
    if(!list) {
        return;
    }
    list->next = list;
    list->prev = list;
}

static inline void *_memset(void *dst, int c, size_t sz)
{
    if(!dst) {
        return QNULL;
    }
    uint8_t *p = (uint8_t *)dst;

    while((uintptr_t)p % sizeof(size_t) != 0 && sz > 0) {
        *p++ = c;
        sz--;
    }

    if(sz >= sizeof(size_t)) {
        size_t word = 0;
        for(size_t i = 0; i < sizeof(size_t); i++) {
            word = (word << 8) | c;
        }

        size_t *p_word = (size_t *)p;
        while(sz >= sizeof(size_t)) {
            *p_word++ = word;
            sz -= sizeof(size_t);
        }

        p = (uint8_t *)p_word;
    }

    while(sz--) {
        *p++ = c;
    }

    return dst;
}

static void *_memcpy(void *dst, const void *src, size_t sz)
{
    if(!dst || !src) {
        return QNULL;
    }
    uint8_t *d = (uint8_t *)dst;
    const uint8_t *s = (const uint8_t *)src;

    while(((uintptr_t)d % sizeof(size_t) != 0 ||
        (uintptr_t)s % sizeof(size_t) != 0) && sz > 0) {
        *d++ = *s++;
        sz--;
    }

    if(sz >= sizeof(size_t)) {
        size_t *d_word = (size_t *)d;
        const size_t *s_word = (const size_t *)s;
        while(sz >= sizeof(size_t)) {
            *d_word++ = *s_word++;
            sz -= sizeof(size_t);
        }
        d = (uint8_t *)d_word;
        s = (const uint8_t *)s_word;
    }

    while(sz--) {
        *d++ = *s++;
    }

    return dst;
}

int qlink_init(QLink *link, uint8_t *dbuf, int(*send)(const uint8_t *, int), int(*recv)(uint8_t *, int))
{
    if(!link || !dbuf || !send || !recv) {
        return -1;
    }
    _memset(link, 0, sizeof(QLink));
    _list_init(&link->sub_list);
    _list_init(&link->ack_list);
    link->send = send;
    link->recv = recv;
    link->dbuf = dbuf;
    return 0;
}

int qlink_sub(QLink *link, QLinkTopic *topic, const char *name, int (*cb)(void *, int), QLinkIsAck isack)
{
    if(!link || !topic || !name || !cb) {
        return -1;
    }
    topic->name = name;
    topic->id = _id_calc(name);
    topic->callback = cb;
    topic->isack = isack;
    _list_insert(&link->sub_list, &topic->sub_node);
    link->topic_num++;
    return 0;
}

int qlink_pack(uint8_t *buf, const char *topic, const uint8_t *data, uint32_t size, uint8_t stamp)
{
    if(!buf || !topic || !data) {
        return -1;
    }

    uint16_t id = _id_calc(topic);
    buf[0] = QLINK_PACK_HEAD[0];
    buf[1] = QLINK_PACK_HEAD[1];
    buf[2] = stamp;
    buf[3] = (uint8_t)((id >> 8) & 0xff);
    buf[4] = (uint8_t)(id & 0xff);
    buf[5] = size;

    uint16_t crc = 0;

    if((size == 0) || (data == QNULL)) {
        crc = _crc16(buf + 2, 4);
    } else {
        _memcpy(buf + 6, data, size);
        crc = _crc16(data, size) + buf[2] + buf[3] + buf[4] + buf[5];
    }
    buf[size + 6] = (uint8_t)(crc & 0xff);
    buf[size + 7] = (uint8_t)((crc >> 8) & 0xff);

    return size + 8;
}

int qlink_pub(QLink *link, uint8_t *buf, const char *topic, const uint8_t *data, uint8_t size)
{
    if(!link || !buf || !topic) {
        return -1;
    }

    if(qlink_pack(buf, topic, data, size, link->stamp) < 0) {
        return -1;
    }

    link->stamp++;
    link->transmissions++;
    return link->send(buf, size + 8);
}

int qlink_ack_init(QLinkAck *ack, const char *topic, uint32_t timeout, uint32_t retry, void (*timeout_cb)(void), uint8_t *buf, uint8_t bufsz)
{
    if(!ack || !topic || !timeout_cb || !buf) {
        return -1;
    }
    ack->name = topic;
    ack->id = _id_calc(topic);
    ack->nack_max = timeout;
    ack->retry = retry;
    ack->retry_cnt = 0;
    ack->nack_cnt = 0;
    ack->timeout_cb = timeout_cb;
    ack->buf = buf;
    ack->bufsz = bufsz;
    return 0;
}

int qlink_ack(QLink *link, uint8_t *buf)
{
    if(!link || !buf) {
        return -1;
    }
    QLinkList *node;
    QLinkAck *ack;
    QLinkList *node_safe;
    QLINK_ITERATOR_SAFE(node, node_safe, &link->ack_list) {
        ack = QLINK_ENTRY(node, QLinkAck, ack_node);
        if(ack->nack_cnt++ >= ack->nack_max) {
            ack->timeout_cb();
            ack->nack_cnt = 0;
            ack->retry_cnt++;
            if(ack->retry_cnt >= ack->retry) {
                _list_remove(&ack->ack_node);
                link->ack_num--;
            }
            continue;
        } else {
            qlink_pub(link, buf, ack->name, ack->buf, ack->bufsz);
        }
    }
    return 0;
}

int qlink_pub_ack(QLink *link, uint8_t *buf, const char *topic, const uint8_t *data, uint8_t size, QLinkAck *ack)
{
    if(!link || !buf || !topic || !data || !ack) {
        return -1;
    }

    if(ack->bufsz < size) {
        return -1;
    }

    _memcpy(ack->buf, data, size);
    ack->nack_cnt = 0;
    ack->retry_cnt = 0;

    if(qlink_pack(buf, topic, data, size, link->stamp) < 0) {
        return -1;
    }
    link->stamp++;
    _list_insert(&link->ack_list, &ack->ack_node);
    link->ack_num++;
    link->transmissions++;
    return link->send(buf, size + 8);
}

QLinkUnpackStatus qlink_unpack_only(QLink *link, const uint8_t *data, uint32_t size, uint16_t *id, uint8_t *unpacked, uint8_t *rsz, uint8_t *stamp)
{
    if(!link || !data || !id || !unpacked || !rsz || !stamp) {
        return QLINK_UNPACK_ERROR;
    }

    for(uint32_t i = 0; i < size; i++) {
        switch(link->recvcnt) {
        case 0:
            if(data[i] == QLINK_PACK_HEAD[0]) {
                link->recvcnt++;
            }
            break;
        case 1:
            if(data[i] == QLINK_PACK_HEAD[1]) {
                link->recvcnt++;
            } else {
                link->recvcnt = 0;
            }
            break;
        case 2:
            *stamp = data[i];
            link->recvcnt++;
            break;
        case 3:
            *id = (uint16_t)data[i] << 8;
            link->recvcnt++;
            break;
        case 4:
            *id |= data[i];
            link->recvcnt++;
            break;
        case 5:
            *rsz = data[i];
            link->recvcnt++;
            break;

        default:
            if(link->recvcnt > 5 && link->recvcnt <= (*rsz + 5)) {
                unpacked[link->recvcnt - 6] = data[i];
                link->recvcnt++;
            } else if(link->recvcnt == (*rsz + 6)) {
                link->crc = (uint16_t)data[i] & 0x00ff;
                link->recvcnt++;
            } else if(link->recvcnt == (*rsz + 7)) {
                uint16_t check = 0;
                link->crc |= (uint16_t)data[i] << 8;
                if(*rsz == 0) {
                    uint8_t tmp[4] = { *stamp, (uint8_t)((*id >> 8) & 0xff), (uint8_t)(*id & 0xff), *rsz };
                    check = _crc16(tmp, 4);
                } else {
                    check = _crc16(unpacked, *rsz) + *stamp + (uint8_t)((*id >> 8) & 0xff) + (uint8_t)(*id & 0xff) + *rsz;
                }
                if(check != link->crc) {
                    link->epackets++;
                    link->recvcnt = 0;
                    *rsz = 0;
                    return QLINK_UNPACK_ECRC;
                } else {
                    link->rpackets++;
                    link->recvcnt = 0;
                    return QLINK_UNPACK_OK;
                }
            } else {
                link->recvcnt = 0;
                *rsz = 0;
            }
            break;
        }
    }
    return QLINK_UNPACK_DECODING;
}

QLinkUnpackStatus qlink_unpack(QLink *link, const uint8_t *data, uint32_t size, int *cb_status, uint8_t *stamp)
{
    if(!link || !data || !cb_status || !stamp) {
        return QLINK_UNPACK_ERROR;
    }

    uint16_t id = 0;
    uint8_t rsz = 0;

    QLinkUnpackStatus status = qlink_unpack_only(link, data, size, &id, link->dbuf, &rsz, stamp);

    if(status == QLINK_UNPACK_OK) {
        if(id == 0x5cb4) { // ID 0x5cb4: ack topic
            QLinkList *ack_node;
            QLinkList *ack_safe;
            QLinkAck *ack;
            uint32_t ack_times = 0;
            uint16_t ack_id = ((uint16_t)link->dbuf[0] << 8) | link->dbuf[1];
            QLINK_ITERATOR_SAFE(ack_node, ack_safe, &link->ack_list) {
                if(ack_times++ > link->ack_num) {
                    break;
                }
                ack = QLINK_ENTRY(ack_node, QLinkAck, ack_node);
                if(ack_id == ack->id) {
                    ack->nack_cnt = 0;
                    _list_remove(&ack->ack_node);
                    link->ack_num--;
                    break;
                }
            }
        } else {
            QLinkList *node;
            QLinkList *node_safe;
            QLinkTopic *topic;
            uint32_t times = 0;
            QLINK_ITERATOR_SAFE(node, node_safe, &link->sub_list) {
                if(times++ > link->topic_num) {
                    break;
                }
                topic = QLINK_ENTRY(node, QLinkTopic, sub_node);
                if(topic->id == id) {
                    if(topic->isack == QLINK_ACK) {
                        uint8_t ack_buf[9] = { 0 };
                        uint8_t id_data[2] = { (uint8_t)((topic->id >> 8) & 0xff), (uint8_t)(topic->id & 0xff) };
                        if(qlink_pub(link, ack_buf, "ack", id_data, 2) <= 0) {
                            status = QLINK_UNPACK_EACK;
                        }
                    }
                    *cb_status = topic->callback(link->dbuf, rsz);
                    return status;
                }
            }
            status = QLINK_UNPACK_NOTSUBSCRIBE;
        }
    }
    return status;
}

int qlink_sub_isack(QLink *link, const char *topic, QLinkIsAck isack)
{
    if(!link || !topic) {
        return -1;
    }

    QLinkList *_node;
    QLinkTopic *_topic;
    uint16_t id = _id_calc(topic);
    QLINK_ITERATOR(_node, &link->sub_list) {
        _topic = QLINK_ENTRY(_node, QLinkTopic, sub_node);
        if(_topic->id == id) {
            _topic->isack = isack;
            return 0;
        } else {
            continue;
        }
    }
    return -1;
}

uint32_t qlink_rpackets(QLink *link)
{
    return link->rpackets;
}

uint32_t qlink_epackets(QLink *link)
{
    return link->epackets;
}

uint32_t qlink_transmissions(QLink *link)
{
    return link->transmissions;
}
